#ifndef EQUIDISTANTCAMERA_H
#define EQUIDISTANTCAMERA_H

#include <opencv2/core/core.hpp>
#include <string>

#include "Camera.h"

namespace camodocal {

/**
 * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method
 * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006
 */

class EquidistantCamera : public Camera {
public:
  class Parameters : public Camera::Parameters {
  public:
    Parameters();
    Parameters(const std::string& cameraName,
               int                w,
               int                h,
               double             k2,
               double             k3,
               double             k4,
               double             k5,
               double             mu,
               double             mv,
               double             u0,
               double             v0);

    double& k2(void);
    double& k3(void);
    double& k4(void);
    double& k5(void);
    double& mu(void);
    double& mv(void);
    double& u0(void);
    double& v0(void);

    double k2(void) const;
    double k3(void) const;
    double k4(void) const;
    double k5(void) const;
    double mu(void) const;
    double mv(void) const;
    double u0(void) const;
    double v0(void) const;

    bool readFromYamlFile(const std::string& filename);
    void writeToYamlFile(const std::string& filename) const;

    Parameters&          operator=(const Parameters& other);
    friend std::ostream& operator<<(std::ostream& out, const Parameters& params);

  private:
    // projection
    double m_k2;
    double m_k3;
    double m_k4;
    double m_k5;

    double m_mu;
    double m_mv;
    double m_u0;
    double m_v0;
  };

  EquidistantCamera();

  /**
    * \brief Constructor from the projection model parameters
    */
  EquidistantCamera(const std::string& cameraName,
                    int                imageWidth,
                    int                imageHeight,
                    double             k2,
                    double             k3,
                    double             k4,
                    double             k5,
                    double             mu,
                    double             mv,
                    double             u0,
                    double             v0);
  /**
    * \brief Constructor from the projection model parameters
    */
  EquidistantCamera(const Parameters& params);

  Camera::ModelType  modelType(void) const;
  const std::string& cameraName(void) const;
  int                imageWidth(void) const;
  int                imageHeight(void) const;

  void estimateIntrinsics(const cv::Size&                              boardSize,
                          const std::vector<std::vector<cv::Point3f>>& objectPoints,
                          const std::vector<std::vector<cv::Point2f>>& imagePoints);

  // Lift points from the image plane to the sphere
  virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
  //%output P

  // Lift points from the image plane to the projective space
  void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
  //%output P

  // Projects 3D points to the image plane (Pi function)
  void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
  //%output p

  // Projects 3D points to the image plane (Pi function)
  // and calculates jacobian
  void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix<double, 2, 3>& J) const;
  //%output p
  //%output J

  void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
  //%output p

  template<typename T>
  static void spaceToPlane(const T* const                params,
                           const T* const                q,
                           const T* const                t,
                           const Eigen::Matrix<T, 3, 1>& P,
                           Eigen::Matrix<T, 2, 1>&       p);

  void    initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
  cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;

  int parameterCount(void) const;

  const Parameters& getParameters(void) const;
  void              setParameters(const Parameters& parameters);

  void readParameters(const std::vector<double>& parameterVec);
  void writeParameters(std::vector<double>& parameterVec) const;

  void writeParametersToYamlFile(const std::string& filename) const;

  std::string parametersToString(void) const;

private:
  template<typename T>
  static T r(T k2, T k3, T k4, T k5, T theta);

  void fitOddPoly(const std::vector<double>& x, const std::vector<double>& y, int n, std::vector<double>& coeffs) const;

  void backprojectSymmetric(const Eigen::Vector2d& p_u,
                            double&                theta,
                            double&                phi) const;

  Parameters mParameters;

  double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
};

typedef std::shared_ptr<EquidistantCamera>       EquidistantCameraPtr;
typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;

template<typename T>
T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
  // k1 = 1
  return theta +
         k2 * theta * theta * theta +
         k3 * theta * theta * theta * theta * theta +
         k4 * theta * theta * theta * theta * theta * theta * theta +
         k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;
}

} // namespace camodocal

#endif
